#include <IRQ.h>
#include <iostream>
#include <types.h>
#include <stddef.h>
#include <IO.h>

dword IRQ::Handlers[16];
word IRQ::Mask;

void IRQ::Init() {
	outb(0x20, 0x11);
	outb(0xA0, 0x11);
	outb(0x21, 0x20);
	outb(0xA1, 0x28);
	outb(0x21, 4);
	outb(0xA1, 2);
	
	outb(0x21, 1);
	outb(0xA1, 1);
	
	outb(0x21, 0xFF);
	outb(0xA1, 0xFF);
	
	Mask = 0xFFFF;
	Update();

	Enable(2);
}

void IRQ::Enable(dword n, bool slave) {
	if (n == 0xFFFFFFFF) {
		Mask = 0;
		Update();
		return;
	}
	
	if (!slave) {
		Mask &= ~(1 << n);
	} else {
		Mask &= ~(1 << (n + 8));
	}
	Update();
}

void IRQ::Disable(dword n, bool slave) {
	if (n == 0xFFFFFFFF) {
		Mask = 0xFFFF;
		Update();
		return;
	}
	
	if (!slave) {
		Mask |= 1 << n;
	} else {
		Mask |= 1 << (n + 8);
	}
	Update();
}

void IRQ::Update() {
	outb(0x21, Mask & 0xFF);
	outb(0xA1, Mask >> 8);
}

void IRQ::Register(dword n, dword func) {
	Handlers[n] = func;
}

void IRQ::Unregister(dword n) {
	Handlers[n] = NULL;
}

CPUContext *IRQ::Exec(CPUContext *ctx) {
	if (Handlers[ctx->int_no - 0x20]) {
		void (*f)(CPUContext *);
		f = (void (*)(CPUContext *)) Handlers[ctx->int_no - 0x20];
		f(ctx);
	}

	if (ctx->int_no - 0x20 >= 8)
		outb(0xA0, 0x20);
		
	outb(0x20, 0x20);
	return ctx;
}
